% IHMS GHR 2024-03-21	该程序为 SSVEP 在线实验程序
%                       该程序使用 FBCCA 算法识别命令，并做出可靠性分析
% IHMS Ghr 2023-03-02
% Demo : online receive data stream


% 《《==================== 用于清除所有端口 ====================》》
% scoms = instrfindall;
% stopasync(scoms);
% fclose(scoms);
% delete(scoms);

close all; clear; clc;
% 创建保存数据文件夹  % Ghr 2023-03-06
if ~exist([pwd,'\Recorder_data'],'file')
	mkdir([pwd,'\Recorder_data\']);
end
if ~exist([pwd,'\Recorder_log'],'file')
	mkdir([pwd,'\Recorder_log\']);
end
EEG_Record = [];
Command_Record = [];
Value_Record = [];
Cost_Record = [];


% 《《==================== 参数初始化 ====================》》
% FBCCA 参数设置
n = 7;

ChannelNumber = 8;  % 通道数
SampleRate = 1000;  % 采样频率（Hz）
TimeWindow = 3;  % 窗口长度（s） ***
DownSampleFactor = 2;  % 降采样因子 ***
CorrectionFactor = 46;  % 基线修正系数 ***
FrequencySample = SampleRate / DownSampleFactor;
WindowLength = TimeWindow * SampleRate;
WindowDownsample = TimeWindow * FrequencySample;
StimulationFrequency = [8.25, 8.75, 9.25, 9.75, 10.25, 10.75, 11.25, 12.25, 13, 12.5];  % 刺激频率 ***
CommandList = [1,2,3,4,5,6,7,8,9,10];  % 对应命令（刺激频率由低到高） ***
% StimulationFrequency = [8.25, 8.75, 9.25, 9.75, 10.25, 10.75, 12.25, 13, 12.5];  % 刺激频率 ***
% CommandList = [1,2,3,4,5,6,7,8,9];  % 对应命令（刺激频率由低到高） ***
HarmonicNumber = 4;  % 参考信号谐波次数 Nh ***
FilterBankFactor = 4;  % 滤波器组参数 N ***
% 选择相关系数向量处理方式
    CCFlag = 0;  % 取模
%     CCFlag = 1;  % 加权平均
    WeightedMeanFactor = [1.20, 0.55];  % 加权平均参数
% 生成参考信号 Yfk
ReferenceSignal = ReferenceSignalCreator(HarmonicNumber, DownSampleFactor, WindowLength, SampleRate, StimulationFrequency);
Threshold = -0.002;  % 结果置信度阈值（此值越小越倾向于认为命令不可靠）

% 初始化DataServer对象
deviceMontage = {'POz','PO3','PO4','PO5','PO6','Oz','O1','O2','TRG'};
device = 'Neuracle';     % see DataParser for supported devices
ipAddress = '127.0.0.1'; % IP address of the DataSerive in EEG Recorder
serverPort = 8712;       % port of the DataSerive in EEG Recorder
% nChan = numel(deviceMontage);  % number of channel+1
nChan = 9;               % number of channel+1
step = 250;              % 步进长度
timeRingbuffer = 4;      % Second（不能删、不用管）
sampleRate = 1000;

%————————ros节点相关————————%
rosshutdown
rosinit('192.168.0.11')
ip='127.0.0.1';
a=udp(ip, 'LocalPort', 8848, 'InputBufferSize', 4096, 'OutputBufferSize', 4096);
% 创建发布者
interval_pub = rospublisher('/control/velocity_interval', 'your_package/VelocityInterval');
% 创建消息
interval_msg = rosmessage(interval_pub);




BURGER_MAX_LIN_VEL = 0.18;
BURGER_MAX_ANG_VEL = 0.4;   

WAFFLE_MAX_LIN_VEL = 0.18;
WAFFLE_MAX_ANG_VEL = 0.4; 
 
LIN_VEL_STEP_SIZE = 0.03;
ANG_VEL_STEP_SIZE = 0.1;

FOMATION_CHANGE_SIZE = 0.05;  %0.05
MAX_FORMATION_CHANGE = 0.70;

% initial parameters 
Linear_velocity = 0;
Angular_velocity = 0;
Fomation_size = 0;

% formation enlargement and narrow down count
fomation_larger_count = 0;
formation_narrow_count = 0;


% formation evolution
formation_bar_count = 0;
formation_tri_count = 0;
formation_shape = 0; % default is triangle formation

cmdpub = rospublisher('/cmd_user',rostype.geometry_msgs_Twist);
accpub = rospublisher('/pub_test','std_msgs/Int32');
% pause(3) % wait to ensure publisher is setup

% p的历史
p = 0.0;
p1 = 0.0;
p2 = 0.0;
key = 0.0;

%————————代码主体——————————%
% 《《==================== 开始在线程序 ====================》》
% 创建DataSever对象，建立TCP连接
dataServer = DataServer_Online(device, nChan, ipAddress, serverPort, sampleRate, timeRingbuffer);
StartTime = datevec(now);  % 记录开始时间
% 打开DataServer,开始接收上游数据并解析
dataServer.Open();
data_online = [];
figure(1);
TimeRec = [];
TimeRec2 = [];
tic;
% TimeNow = datevec(now);% 记录时间
% t1 = TimeNow(6)
while true
%     while(size(TimeRec,1)<=241)

	if strcmpi(get(gcf,'CurrentCharacter'),'j')  % 跳出循环条件（在图形界面中，按键盘“j”）
        break;
	end
	data_now = dataServer.SaveEEGData_Online();
%     EEG_Record = cat(2, EEG_Record, data_now);
	data_online = cat(2, data_online, data_now);
	if (size(data_online, 2) < WindowLength)
        pause(0.01);
	else
        EEGDataNow = data_online(:, 1:WindowLength);
        % 执行 FBCCA 算法，输出命令和相关系数矩阵
        [outputCommand, ~, ~, correlationCoefficient2, costNow] = FBCCA_algorithm(EEGDataNow, ReferenceSignal, ...
            ChannelNumber, DownSampleFactor, CorrectionFactor, FrequencySample, WindowDownsample, StimulationFrequency, ...
            CommandList, FilterBankFactor, CCFlag, WeightedMeanFactor, Threshold);
        p = outputCommand;
        Command_Record = cat(2, Command_Record, outputCommand);  % 记录命令
%         Value_Record = cat(2, Value_Record, correlationCoefficient2);  % 记录相关系数
%         Cost_Record = cat(2, Cost_Record, costNow);  % 记录可靠性分析（ costNow(3) = 1 则认为不可靠）
        disp(outputCommand);
        data_online(:, 1:step) = [];
        toc;
        TimeRec = [TimeRec;toc;];
%         TimeNow = datevec(now);% 记录时间
%         t1 = TimeNow(6)
%         TimeRec2 = [TimeRec2;t1;];
        tic;
%         TimeNow = datevec(now);% 记录时间
%         t1 = TimeNow(6)
%         TimeRec2 = [TimeRec2;t1;];


if p==2 % 加速
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change
        Linear_velocity = LinearConstrain(Linear_velocity + LIN_VEL_STEP_SIZE, BURGER_MAX_LIN_VEL);
        Angular_velocity = 0.0;

    elseif p==1 % 队形缩小 
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = formation_narrow_count + 1;
        if formation_narrow_count >= 4
            formation_narrow_count = 0;
            Fomation_size = FormationConstrain(Fomation_size - FOMATION_CHANGE_SIZE, MAX_FORMATION_CHANGE);
        end
        % velocity change

    elseif p==3 % 队形变成三角形
        % formation evolution
        formation_tri_count = formation_tri_count + 1;
        formation_bar_count = 0;
        if formation_tri_count >= 6
            formation_tri_count = 0;
            formation_shape = 0;
        end
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change


    elseif p==4 % 左转
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change
        Angular_velocity = AngularConstrain(Angular_velocity + ANG_VEL_STEP_SIZE, BURGER_MAX_ANG_VEL);

    elseif p == 5 % 停止，退出自动
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change
        Linear_velocity = 0.0;
        Angular_velocity = 0.0;

    elseif p==6 % 右转
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change
        Angular_velocity = AngularConstrain(Angular_velocity - ANG_VEL_STEP_SIZE, BURGER_MAX_ANG_VEL);

    elseif p==8  % 队形放大
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = formation_narrow_count + 1;
        if formation_narrow_count >= 4
            formation_narrow_count = 0;
            Fomation_size = FormationConstrain(Fomation_size + FOMATION_CHANGE_SIZE, MAX_FORMATION_CHANGE);
        end
        % velocity change     

    elseif p==9 % 减速
        % formation evolution
        formation_bar_count = 0;
        formation_tri_count = 0;
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;



        % velocity change
        Linear_velocity = LinearConstrain(Linear_velocity - LIN_VEL_STEP_SIZE, BURGER_MAX_LIN_VEL);
        Angular_velocity = 0.0;

    elseif p == 10 % 队形变为条形
        % formation evolution
        formation_bar_count = formation_bar_count + 1;
        formation_tri_count = 0;
        if formation_bar_count >= 6
            formation_bar_count = 0;
            formation_shape = 1;
        end
        % formation change
        fomation_larger_count = 0;
        formation_narrow_count = 0;
        % velocity change

   end
    

%     Linear_velocity 
%     Angular_velocity
    
%     cmdpub = rospublisher('/cmd_udp',rostype.geometry_msgs_Twist);
%   发布命令 
    cmdmsg = rosmessage(cmdpub);
    cmdmsg.Linear.X = Linear_velocity;
    cmdmsg.Linear.Y = Fomation_size;
    cmdmsg.Linear.Z = 0;
    cmdmsg.Angular.X = formation_shape;
    cmdmsg.Angular.Z = Angular_velocity;
    send(cmdpub,cmdmsg)
    disp('速度消息已发布');

% 发布命令区间--模糊逻辑
    interval_msg.VMin = v_min;
    interval_msg.VMax = v_max;
    interval_msg.WMin = w_min;
    interval_msg.WMax = w_max;
    interval_msg.LMin = L_min;
    interval_msg.LMax = L_max;
    send(interval_pub, interval_msg);
    disp('区间消息已发布');















    
%   用于测试准确率
    accmsg = rosmessage(accpub);
    accmsg.Data = p;
    send(accpub,accmsg);
     
    
%     pause(0.1)

    end


end

% nn = 0;
% for i = 1:size(Command_Record,1)
%     if Command_Record(i) == n
%         nn = nn+1;
%     end
% end
% Acc = nn/size(Command_Record,1);


% 《《==================== 数据保存 ====================》》
dataServer.Close();  % 关闭DataServer,停止接收数据
SaveTime = datestr(now,'yyyymmddHHMMSS');  % 记录时间
StopTime = datevec(now);  % 记录结束时间
ExperimentTime = [StartTime; StopTime];
StartTime_p = [num2str(StartTime(1)),'年',num2str(StartTime(2)),'月',num2str(StartTime(3)),'日',num2str(StartTime(4)),'时',num2str(StartTime(5)),'分',num2str(StartTime(6)),'秒']
StopTime_p = [num2str(StopTime(1)),'年',num2str(StopTime(2)),'月',num2str(StopTime(3)),'日',num2str(StopTime(4)),'时',num2str(StopTime(5)),'分',num2str(StopTime(6)),'秒']

TT = (sum(TimeRec)-2)/size(TimeRec,1)
nn = 0;
for i = 1:size(Command_Record,2)
    if Command_Record(i) == n
        nn = nn + 1;
    end
end
Acc = nn/size(Command_Record,2)

% 保存数据到指定文件夹  % Ghr 2023-03-06
EEGfilename = ['EEGData_' SaveTime];
% save([pwd,'\Recorder_data\',EEGfilename],'EEG_Record','Command_Record','Value_Record','Cost_Record','ExperimentTime','TimeRec', 'n', 'TT', 'Acc');
logname = ['log_' SaveTime];
diary([pwd,'\Recorder_log\',logname]);
disp('All data has been saved !');
close all;



% 《《==================== 函数体 ====================》》
% 生成参考信号 Yfk *
function Referencesignal = ReferenceSignalCreator(nh, dw, win, sr, fk)
    Y = zeros(size(fk,2), nh*2, win/dw);
    t = (0:dw:win-1)/sr;
    for i = 1:size(fk,2)
        for j = 1:nh
            Y(i, 2*j-1, :) = sin(2*pi*j*fk(i)*t);
            Y(i, 2*j, :) = cos(2*pi*j*fk(i)*t);
        end
    end
    Referencesignal = single(Y);
end

